#!/usr/bin/env python

# This ROS node listens for Polyhedron messages on the topic that you specify.
# It keeps a list of recently reported Convex_Space objects, and tries to
# calculate their intersections.
#
# The trivial intersection for any two spaces is simply the union of their
# sets of planes, but we are going to do more work than that, for several
# reasons.  First, we want to be able to detect (and thus not report) spaces
# which have 0 volume (meaning that the input spaces don't actually intersect).
# Second, one of our key jobs will be to generate an axis-aligned bounding box
# for each object, and we need the real, practical bounds for that.
#
# Our intersection algorithm will be based on the idea of convex sets, as
# discussed in ua_objects/intersector/lecconset.pdf.  The idea is that any
# (finite) convex set can be defined by a set of points; the convex hull
# which these points define is the set of all points "inside" them.  The
# way I like to think of it is that the points are the corners of a die,
# and you can find out the shape by "rolling" the die across a plane.
# Each face is made up of 3 or more points.  Of course, most combinations of
# points don't result in exterior faces; most slice through the interior of
# the space.  Thus, a place which contains one face is a plane which goes
# through 3 or more points, and all of the remaining points are on one side
# of the plane.
#
# NOTE: The formal definition of a convex set, as defined in lecconset.pdf,
# allows for redundant interior points.  We will discard any such points that
# we find.  The only points that we retain will be the ones which are actually
# vertices on the outside of the die.
#
# Unfortunately, lecconset.pdf didn't give an algorithm for calculating
# the intersection of connected sets, so I will develop one here.  I will
# first describe it on the assumption that a single plane is slicing a
# finite convex space; we can easily build up a loop which slices an existing
# space by all of the planes of another.  I will then generalize the
# algorithm to consider infinite spaces.  Finally, I will make some comments
# about deciding when to cull planes from the plane list.
#
# PLANE INTERSECTS A FINITE CONVEX SET
#
# In this algorithm, we have a finite convex set, defined by a set of
# points S.  When a single plane slices this space, we will check each
# point in the set, classifying the point as "in" or "out".  Points
# which are "in" will be in the output set O.  Points which are "out"
# will not be in the output.  However, we need to find a set of points, all
# lying on the slicing plane, which define the new face which is
# created.
#
# First, we will handle the trivial cases.  If *all* of the points are "in",
# then it means that the plane is redundant; it does not slice anything
# from the previous space.  So, our ouput set of points is the same as the
# input, and this plane will be discarded.  On the other hand, if all of
# the points are "out", then it means that the plane slices away the entire
# space, leaving us 0 volume.  This means that our space has died, and we can
# halt the algorithm.
#
# In general, however, we expect to have some points which are in, and some
# which are out.  We note that the new face will be defined by a new set of
# points, all lying on the slicing plane; we also note that each vertex on
# this new face will lie on an old edge; that is, it will lie on some edge
# which reaches from an "in" point to an "out" point.
#
# In our current version of the code, I don't have any record of which
# edges exist, so I generate a list of potential edges by running a line
# between each in/out pair.  I calculate the location where this hits the
# plane, and count this as the new point.
#
# Now, I know that most points that I generate in this manner are
# redundant; they lie *inside* the face, instead of at some corner.
#
# It helps to think of this (initially) as a 2D problem.  The plane is a 2D
# space, and all of the points lie on it (technically, because of rounding
# issues, they all lie *near* it, but we'll ignore that for now).  Now imagine
# two points A,B which are adjacent to each other on the perimeter of the new
# face.  That is, the line segment between these two points is one of the edges
# in the new polyhedron.  Extend that line segment into a full line, and you
# will see that all of the other points in the face fall on one side of that
# line.  Algorithmically, you could calculate the a vector perpendicular to the
# line, and calculate the dot product between that vector and the vectors to
# each other point in the set; all of the dot products would be positive (if
# the perpendicular vector points into the face) or negative (if it points
# away).
#
# Of course, we can accomplish the same in 3D space.  We first calculate the
# normal of the plane, and then calculate the vector between the two points
# B-A.  The cross product of those two is a vector which lines on the slicing
# plane, perpendicular to the vector between the two points.  We can then
# dot product this with the various vectors to the other points to see if this
# pair of points matches the requirement (that all the dot products are either
# positive or negative).
#
# So, to find the points on the perimeter, we first do an O(n^2) search
# amongst the points until we find some pair which satisfies the condition
# above.  Then, we trace around the perimeter in order; for each point on
# the perimeter, we'll find the next point using cross products.  When we
# finally find that the next point is the first point A, we have found the
# entire loop, and all other points can be discarded.
#
# INFINITE SPACES
#
# NOTE: I'm not yet going to consider infinite spaces in the algorithm,
# but this section details my thoughts about the matter.  I hope to add
# it later.  Right now, I will just start every calculation with a
# simple bounding box with very large extent, and we'll pretend that it
# is at infinity.
#
# ...
#
# We can imagine an infinite space as a normal convex space, which is
# defined by one or more points at infinity.  (The complete 3d space can
# be defined by 8 points, arranged in a cube of infinite extent.)
#
# Of course, a point at infinity doesn't have a finite specifiable
# location; nor is it just a direction.  Think about the space defined
# by the intersection of two planes; there is a line, which defines the
# "fold" between them, and then an infinite 3D space reaching out from
# there.  Note that there is one dimension (parallel to the line) where
# the space is infinite in both directions; in the other two dimensions,
# it is infinite in one direction and finite in another.  You can see
# that the space can be defined by six points at infinity; two on the
# line, and two on each plane.
#
# You can understand, thinking about those two points on the line, that
# a point at infinity is defined by *BOTH* a point (anywhere on the line)
# *AND* a direction.  A point at infinity, then, is really more like a
# ray than a point; when a point slices that ray, it may face towards the
# infinite direction (in which case we move the base point, but it's still
# a point at infinity) or towards the ray (in which case the ray is sliced
# off and becomes a line segment...that is, two points).
#
# CULLING PLANES
#
# My slice() algorithm already removes planes from the set when they are
# redundant at the moment they are placed.  That is, if I use a plane to
# slice a space, and all of the points are "in" the half-space defined by
# the plane, then I won't save that plane.
#
# However, I do not go back and reconsider old planes.  A plane, once in
# the list, is never removed, even if it happens to become redundant
# later.  I'd like to fix that, but I haven't, for now.



import roslib; roslib.load_manifest('intersector')
import rospy
import copy

import intersector.msg
from intersector.msg import Convex_Space
from std_msgs.msg import String

from math import sqrt

from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point



def inclusive_range(start,end, step=1):
    assert step != 0
    if step > 0:
        return range(start, end+1, step)
    else:
        return range(start, end-1, step)


# I'm sure there's a Python function for this, but I don't know what it is.
# This is a function which formats an array, expanding the member elements,
# rather than just leaving references to the class instances.
def exp_str(input_array, depth=1):
    if depth == 0:
        return str(input_array)

    if(len(input_array) == 0):
        return "[]"

    retval = "[0=" + exp_str(input_array[0], depth-1)
    for i in range(1,len(input_array)):
        elem = input_array[i]
        retval += ", %d=%s" % (i, exp_str(elem, depth-1))
    return retval + "]"



class Vector3:
    def __init__(self, x,y,z):
        self.x = x
        self.y = y
        self.z = z
    def __str__(self):
        return "Vector3:[ x=%f y=%f z=%f ]" % (self.x, self.y, self.z)



    def mag(self):
        return sqrt(self.magSqrd())
    def magSqrd(self):
        return self.dotProd(self)

    def normalize(self):
        return self / self.mag()


    def __eq__(self, other):
#        print "Vector3:__eq__()"
        return self.x == other.x and self.y == other.y and self.z == other.z

    def __lt__(self, other):
        def internal():
            if(self.x < other.x):
                return True
            if(self.x > other.x):
                return False

            if(self.y < other.y):
                return True
            if(self.y > other.y):
                return False

            if(self.z < other.z):
                return True

            return False

#        print "Vector3:__lt__(): self=%s other=%s" % (self,other)
        retval = internal()
#        print "Vector3:__lt__(): retval=%d" % retval
        return retval

    def __le__(self, other):
#        print "Vector3:__eq__()"
        return (self == other) or (self < other)

    def __ne__(self, other):
#        print "Vector3:__ne__()"
        return not (self == other)

    def __gt__(self, other):
#        print "Vector3:__gt__()"
        return not (other <= self)

    def __ge__(self, other):
#        print "Vector3:__ge__()"
        return not (other < self)



    def __add__(self, other):
        return Vector3(self.x + other.x, self.y + other.y, self.z + other.z)
    def __sub__(self, other):
        return Vector3(self.x - other.x, self.y - other.y, self.z - other.z)
    def __mul__(self, multiplicand):
        return Vector3(self.x * multiplicand, self.y * multiplicand, self.z * multiplicand)
    def __rmul__(self, other):
        return self * other
    def __div__(self, dividend):
        return Vector3(self.x / dividend, self.y / dividend, self.z / dividend)



    def dotProd(self, other):
        return (self.x * other.x) + (self.y * other.y) + (self.z * other.z)
    def crossProd(self, other):
        return Vector3(   self.y * other.z - other.y * self.z,
                        - self.x * other.z + other.x * self.z,
                          self.x * other.y - other.x * self.y)


    @staticmethod
    def selfTest():
        x = Vector3(1,0,0)
        y = Vector3(0,1,0)
        z = Vector3(0,0,1)

        assert(x == x)
        assert(y == y)
        assert(z == z)

        assert(x != y)
        assert(x != z)
        assert(y != x)
        assert(y != z)
        assert(z != x)
        assert(z != y)

        assert(x+x == Vector3(2,0,0))
        assert(x+y == Vector3(1,1,0))
        assert(x+z == Vector3(1,0,1))
        assert(y+x == Vector3(1,1,0))
        assert(y+y == Vector3(0,2,0))
        assert(y+z == Vector3(0,1,1))
        assert(z+x == Vector3(1,0,1))
        assert(z+y == Vector3(0,1,1))
        assert(z+z == Vector3(0,0,2))

        assert(x*2 == Vector3(2,0,0))
        assert(y*3 == Vector3(0,3,0))
        assert(z*4 == Vector3(0,0,4))

        assert(5*x == Vector3(5,0,0))
        assert(6*y == Vector3(0,6,0))
        assert(7*z == Vector3(0,0,7))

        assert( 8*x +  9*y == Vector3( 8, 9, 0))
        assert(10*x + 11*z == Vector3(10, 0,11))
        assert(12*y + 13*z == Vector3( 0,12,13))

        assert(14*x - 15*y == Vector3(14,-15,  0))
        assert(16*x - 17*z == Vector3(16,  0,-17))
        assert(18*y - 19*z == Vector3( 0, 18,-19))

        assert(x.dotProd(y) == 0);
        assert(x.dotProd(z) == 0);
        assert(y.dotProd(z) == 0);

        assert(x.dotProd(x) == 1);
        assert(y.dotProd(y) == 1);
        assert(z.dotProd(z) == 1);

        assert(x.crossProd(y) == Vector3( 0, 0, 1))
        assert(y.crossProd(x) == Vector3( 0, 0,-1))
        assert(x.crossProd(z) == Vector3( 0,-1, 0))
        assert(z.crossProd(x) == Vector3( 0, 1, 0))
        assert(y.crossProd(z) == Vector3( 1, 0, 0))
        assert(z.crossProd(y) == Vector3(-1, 0, 0))
Vector3.selfTest()



class Intersection:
    # this models the intersection of one or more Convex_Space's.


    def __str__(self):
        return "Intersection:[ convex_spaces=" + exp_str(self.convex_spaces) + " planes=" + exp_str(self.planes) + " points=" + exp_str(self.points) + " ]"


    def __init__(self, convex_space):
        # initialize the list of Convex_Space's (messages from ROS) which make up
        # the input to this space.
        self.convex_spaces = [convex_space]

        # initialize the set of points which define the convex set.  It starts
        # out empty; then we add in 8 points defining a very large cube; then
        # we slice parts of out if with the plane defined in the space object.
        self.planes = []
        self.points = []

        # set up the hack to simulate infinite spaces: start with a very large
        # finite space.  We define 3 pairs of planes, for the 3 dimensions;
        # each of 8 points is built by choosing two of those points
        dist = 10*1000

        for x in         inclusive_range(-1,1,2):
            for y in     inclusive_range(-1,1,2):
                for z in inclusive_range(-1,1,2):
                    self.points.append(Vector3(x*dist, y*dist, z*dist))

#        print "Intersection:__init__(): slicing the initial points with the planes of the convex space"
#        print "                         len(convex_space.planes)=%d convex_space.planes=\n%s" % ( len(convex_space.planes), exp_str(convex_space.planes) )
#        print
        for plane in convex_space.planes:
            self.slice(plane)

#        print "Intersection:__init__(): LOOP DONE!"


    def empty(self):
        return len(self.points) == 0


    def slice(self, plane):
#        print "slice(): PLANE:"
#        print plane
#        print


        # This slices this Intersection with a certain plane.  It modifies the
        # object in place.

#        print "slice(): STARTING POINTS: %s" % exp_str(self.points)
#        print

        pts_in  = []
        pts_out = []

#        print "slice(): SELF:"
#        print self
#        print
#        print "slice(): PLANE:"
#        print plane
#        print

        for pt in self.points:
#            print "slice(): PT:"
#            print pt
#            print "slice(): PLANE:"
#            print plane
#            print plane.a * pt.x + plane.b * pt.y + plane.c * pt.z + plane.d

            if plane.a * pt.x + plane.b * pt.y + plane.c * pt.z + plane.d < 0:
#                print "IN"
                pts_in .append(pt)
            else:
#                print "OUT"
                pts_out.append(pt)

#            print

        if len(pts_in) == 0:
#            print "slice(): Space has become empty, setting points to the empty list"
            self.planes.append(plane)
            self.points = []
            return

        if len(pts_out) == 0:
#            print "slice(): Plane was redundant, skipping"
#            print
            return


        # calculate the normal to the plane, which will be used in all of the
        # tests below.
        normal = Vector3(plane.a, plane.b, plane.c)


#        print "slice(): ABOUT TO GEN CANDIDATES.  pts_in=%s pts_out=%s" % (exp_str(pts_in), exp_str(pts_out))
#        print


        # generate the list of candidate points; each lies on the plane, on
        # segment between an 'in' point and an 'out' point
        candidates = []
        for pt_in in pts_in:
            for pt_out in pts_out:
                # we draw a line through the two points; the parametric
                # version of it t=[0,1] is:
                #     pt_result = pt_in + t(pt_out - pt_in)

                base = pt_in
                vector = pt_out - pt_in

#                print "slice(): CANDIDATE GEN: pt_in=%s pt_out=%s vector=%s" % (pt_in, pt_out, vector)

                # we can solve for t using the equation
                #     a (base.x +   t vector.x) + b (base.y +   t vector.y) + c (base.z +   t vector.z) + d = 0
                #     a  base.x + a t vector.x  + b  base.y + b t vector.y  + c  base.z + c t vector.z  + d = 0
                #                 a t vector.x  +             b t vector.y  +             c t vector.z      =  - a base.x - b base.y - c base.z - d
                #                   t                                                                       = (- a base.x - b base.y - c base.z - d) / (a vector.x + b vector.y + c vector.z)
                abc = Vector3(plane.a, plane.b, plane.c)
                dp  = abc.dotProd(vector)

                # special case handling (bugfix): If both of the points lie on the plane (???rounding error???), then abc.dotProd(vector) can be 0
                if dp == 0:
                    print "ERROR: dp=0"
                    t = 0
                else:
                    t = (- abc.dotProd(base) - plane.d) / dp

                if t < 0:
                    if t < -0.1:  # only report the error if it is significantly off
                        print "ERROR: t=%f.  Normalizing to 0." % t
                    t = 0
                elif t > 1:
                    if t >  1.1:  # only report the error if it is significantly off
                        print "ERROR: t=%f.  Normalizing to 1." % t
                    t = 1

#                print "slice(): PARAMETRIC: abc=%s abc.dotProd(base)=%s plane.d=%f <numerator>=%f abc.dotProd(vector)=%s t=%f" % (abc, abc.dotProd(base), plane.d, -abc.dotProd(base) - plane.d, abc.dotProd(vector), t)
#                print "slice():     RESULT: %s" % (base + t * vector)
#                print

                candidates.append(base + t * vector)


#        print "slice(): CANDIDATES TEMP LIST.  len(candidates)=%d candidates=%s" % ( len(candidates), exp_str(candidates) )
#        print

        candidates = sorted(candidates)

#        print "slice(): SORTED CANDIDATES: len(candidates)=%d candidates=%s" % ( len(candidates), exp_str(candidates) )
#        print

        for i in range(0, len(candidates)-1):
            assert candidates[i] <= candidates[i+1]


#        print "slice(): searching for duplicate points in the candidate list..."
        for i in range(0, len(candidates)-1):
            if candidates[i] == candidates[i+1]:
#                print "slice(): (%d,%d) are duplicates; removing %d" % (i,i+1, i)
                candidates[i] = False

#        print "slice(): len(candidates)=%d candidates=%s" % ( len(candidates), exp_str(candidates) )
#        print

#        print "slice(): removing colinear points..."
        for i in range(0, len(candidates)-2):
            I = candidates[i]
            if I is False:
                continue

            for j in range(i+1, len(candidates)-1):
                J = candidates[j]
                if J is False:
                    continue

                IJ = J-I
                IJ_magSqrd = IJ.magSqrd()
                assert(IJ_magSqrd > 0)

                for k in range(j+1, len(candidates)):
                    K = candidates[k]
                    if K is False:
                        continue

                    IK = K-I
                    IK_magSqrd = IK.magSqrd()
                    assert(IK_magSqrd > 0)

                    JK = K-J
                    JK_magSqrd = JK.magSqrd()
                    assert(JK_magSqrd > 0)

                    # we do a trivial check to make sure that the vectors are
                    # pointed in the same general direction.  This, plus the
                    # magnitude check after it, are designed to give us enough
                    # sanity checking that we can get away with fuzzy_eq() in
                    # the final check; if we don't do these preliminary checks,
                    # then fuzzy_eq() can discard points that are actually on
                    # the edge, for various reasons.
                    #
                    # And, of course, we need fuzzy_eq() because we have to
                    # handle #@$)(*#@$ floating-point rounding errors.

                    def sign(x):
                        if x == 0:
                            return  0
                        if x <  0:
                            return -1
                        else:
                            return  1

                    if sign(IJ.x) != sign(IK.x) or sign(IJ.y) != sign(IK.y) or sign(IJ.z) != sign(IK.z):
#                        print "slice(): colinear check: skipping (%d,%d,%d) because the general directions don't match" % (i,j,k)
                        continue

                    if IJ_magSqrd > IK_magSqrd or JK_magSqrd > IK_magSqrd:
#                        print "slice(): colinear check: skipping (%d,%d,%d) because the lengths aren't correct" % (i,j,k)
                        continue


                    # we use linear scaling to project where the K point
                    # "ought" to be (based on I,J).  Then we use fuzzy_eq()
                    # to compare that to the real K, and see if they match.
                    #
                    # UPDATE: scaled_K can have points which are close to
                    # 0, and since fuzzy_eq() compares ratios, points near
                    # coordinate 0 would have to be very precise.  So, instead
                    # of calculating the position of K, we'll calculate
                    # the predicted vector IK.

                    #scaled_K = I + IJ * sqrt(IK_magSqrd / IJ_magSqrd)
                    scaled_IK = IJ * sqrt(IK_magSqrd / IJ_magSqrd)

                    def fuzzy_eq(a,b):
                        if a == b:
                            return True
                        if a == 0 or b == 0:
                            return False
                        if a < b:
                            return fuzzy_eq(b,a)
                        return (a/b) < 1.000001

                    if fuzzy_eq(scaled_IK.x, IK.x) and fuzzy_eq(scaled_IK.y, IK.y) and fuzzy_eq(scaled_IK.z, IK.z):
#                        print "slice(): (%d,%d,%d) are colinear; removing %d" % (i,j,k, j)
                        candidates[j] = False
#                        print "slice(): CUR CANDIDATES: %s" % exp_str(candidates)
                        break



#        # quick experiments indicate that the loop below (colinear point
#        # elimination) is the slowest part
#        # of the whole algorithm, so I'm looking for ways to speed it up.
#        # One of the ways is to cache up values which I originally generated
#        # multiple times inside the loop.  The original code was clearer, but
#        # hopefully this version will be faster.
#        cache__distances = []
#        for i in range(0, len(candidates)-1):
#
#            # create an entry for [i].  We don't yet know if it will contain
#            # anything, but we need a placeholder for indexing (that is,
#            # indexing into later elements) even if this one won't be used.            
#            cache__distances.append([])
#            assert len(cache__distances) == i+1
#
#            if candidates[i] is False:
#                continue
#
#            # fill up the array with spacers until the point where we actually
#            # might need to use it.
#            for j in range(0,i+1):
#                cache__distances[i].append( None )
#            assert len(cache__distances[i]) == i+1
#
#            for j in range(i+1, len(candidates)):
#                if candidates[j] is False:
#                    cache__distances[i].append( False )
#                else:
#                    cache__distances[i].append( (candidates[j]-candidates[i]).magSqrd() )
#
#                assert len(cache__distances[i]) == j+1
#
#        print "slice(): removing colinear points..."
#        for i in range(0, len(candidates)-2):
#            if candidates[i] is False:
#                continue
#
#            for j in range(i+1, len(candidates)-1):
#                if candidates[i] is False:
#                    break
#                if candidates[j] is False:
#                    continue
#
#                # calculate the difference vector between points i,k.  Then
#                # calculate the perpendicular vector.  This is the same as
#                # will be used later in isPermieterPair().  We'll dot product
#                # this with all of the other points in the set, looking for
#                # colinear ones.
#                perpend = normal.crossProd( candidates[j] - candidates[i] )
#
#                for k in range(j+1, len(candidates)):
#                    if candidates[i] is False or candidates[j] is False:
#                        break
#                    if candidates[k] is False:
#                        continue
#
##                    print "checking: (i,j,k)=(%d,%d,%d) len=%d" % (i,j,k, len(candidates))
#
#                    assert not (candidates[k] is False or candidates[i] is False)
#
#                    diff2 = candidates[k] - candidates[i]
#                    dotProd = perpend.dotProd(diff2)
#
#                    if dotProd == 0:
#                        # i,j,k are colinear.  We'll find the one that is in
#                        # the middle and discard it.  If we actually delete
#                        # it, then things get ugly...we have to worry about
#                        # keeping the indices correct, etc.  Instead, we'll
#                        # just set it to False and delete all of them later!
#                        I = candidates[i]
#                        J = candidates[j]
#                        K = candidates[k]
#                        assert(I != J and I != K and J != K)
#
#                        #IJ_magSqrd = (I-J).magSqrd()
#                        #IK_magSqrd = (I-K).magSqrd()
#                        #JK_magSqrd = (J-K).magSqrd()
#                        IJ_magSqrd = cache__distances[i][j]
#                        IK_magSqrd = cache__distances[i][k]
#                        JK_magSqrd = cache__distances[j][k]
#                        assert(IJ_magSqrd > 0 and IK_magSqrd > 0 and JK_magSqrd > 0)
#
##                        print "i,j,k are colinear!   (i,j,k)=(%d,%d,%d)" % (i,j,k)
##                        print "                    i = %s" % I
##                        print "                    j = %s" % J
##                        print "                    k = %s" % K
##                        print "                    IJ_mag = %f" % IJ_mag
##                        print "                    IK_mag = %f" % IK_mag
##                        print "                    JK_mag = %f" % JK_mag
#
#
#                        if IJ_magSqrd >= IK_magSqrd and IJ_magSqrd >= JK_magSqrd:
##                            print "                    <k is the middle>"
#                            candidates[k] = False
#                        elif IK_magSqrd >= IJ_magSqrd and IK_magSqrd >= JK_magSqrd:
##                            print "                    <j is the middle>"
#                            candidates[j] = False
#                        else:
#                            assert JK_magSqrd >= IJ_magSqrd and JK_magSqrd >= IK_magSqrd
##                            print "                    <i is the middle>"
#                            candidates[i] = False
#
##                        print "                    new: len(candidates)=%d candidates=%s" % ( len(candidates), exp_str(candidates) )
#
#
#                # we've searched all of the k elements, so we can now increment j
#                j += 1
#            # and now we increment i
#            i += 1

#        print "slice(): len(candidates)=%d candidates=%s" % ( len(candidates), exp_str(candidates) )
#        print


#        print "slice(): Removing the deleted elements..."
        i=0
        while i < len(candidates):
            if candidates[i] is False:
                candidates = candidates[:i] + candidates[i+1:]
#                print "                    new: len(candidates)=%d candidates=%s" % ( len(candidates), exp_str(candidates) )
            else:
                i += 1


#        print "slice(): CANDIDATES DONE.  len(candidates)=%d candidates=%s" % ( len(candidates), exp_str(candidates) )
#        print


        # this helper function determines if two points are a pair on the
        # perimeter of the candidate set.  It's used both to find the original
        # pair, and then to find the following pairs after that.
        def isPerimeterPair(A, B):
#            print "slice(): isPerimeterPair(): A=%s" % A
#            print "                            B=%s" % B
#            print "                            len(candidates)=%d candidates=%s" % ( len(candidates), exp_str(candidates) )


            # The callers ensure that A != B, so we don't have to ensure that.

            # this is the perpendicular vector (on the plane) to the line
            perpend = normal.crossProd(B-A)

#            print "                            normal =%s" %  normal
#            print "                            (B-A)  =%s" % (B-A)
#            print "                            perpend=%s" %  perpend

            # loop over all of the *other* points in the candidate set;
            # mark whether we've found positive or negative dot products.
            # Break out of the loop if we eventually find both types.
            pos_found = False
            neg_found = False
            for Pi in candidates:
                if A is Pi or B is Pi:
                    continue

#                print "                              Pi  =%s" % Pi
#                print "                                Pi-A=%s" % (Pi-A)

                # ok, we know that A, B, and Pi are all different.  We now
                # check the dot product, and record if the result is
                # positive or negative.

                dotProd = perpend.dotProd(Pi-A)
#                print "                                dotProd=%e" % dotProd


                # if dotProd == 0, then Pi is colinear with (A-B).  We try to
                # avoid this by detecting colinear points earlier in the process,
                # but it can happen due to rouding errors.  I print a warning but
                # then simply ignore that this point ever existed.
                if dotProd == 0:

#                    print "slice():isPerimeterPair(): WARNING: dotProd==0:"
#                    print "                                    A      =%s" % A
#                    print "                                    B      =%s" % B
#                    print "                                    Pi     =%s" % Pi
#                    print "                                    (Pi-A) =%s" % (Pi-A)
#                    print "                                    normal =%s" % normal
#                    print "                                    (B-A)  =%s" % (B-A)
#                    print "                                    perpend=%s" % perpend

                    continue



#                    assert(False)
#
#
#
#                    print
#                    print
#                    print
#                    print
#                    print "ERROR: dotProd == 0"
#                    print
#                    print
#                    print
#                    print
#                    return 0
#
#
#
#                    print "(B -A)=%s mag=%f" % (  B-A, ( B-A).mag())
#                    print "(Pi-A)=%s mag=%f" % ( Pi-A, (Pi-A).mag())
#                    print "(Pi-B)=%s mag=%f" % ( Pi-B, (Pi-B).mag())
#
#                    if (B-A).mag() > (Pi-A).mag() and (B-A).mag() > (Pi-B).mag():
#                        print "                                Pi is between A and B, continue to consider if A,B are a pair"
#                        continue
#                    else:
#                        print "                                Pi is outside A,B; isPerimeterPair() is immediately aborting."
#                        neg_found = pos_found = True
#                        break


                if dotProd < 0:
                    neg_found = True
                if dotProd > 0:
                    pos_found = True

                if neg_found and pos_found:
                    break

#            print "                            retval=%d" % (neg_found != pos_found)

            return (neg_found != pos_found)
        # end of helper function


        # this is the working set of all of the perimeter points we've found
        # so far.
        perimeter = []

        # hunt for a pair (A,B) on the perimeter.
        for A in candidates:
            for B in candidates:
                if A is B:
                    continue

#                print "slice(): ABOUT TO CHECK A CANDIDATE PAIR: A=%s" % A
#                print "                                          B=%s" % B

                if not isPerimeterPair(A,B):
                    continue

#                print "slice(): PERIMETER PAIR FOUND!"

                # A,B is our first pair.  We will store them, and then we'll
                # work our way out from B.
                perimeter.append(A)
                perimeter.append(B)

                # this saves the current endpoint of the known perimeter.  We
                # could just read the last element in perimeter[] over and
                # over, but this is easier to read.
                endPoint = B

                # we loop until the endPoint goes back to the beginning; note that
                # we also have code to detect the case where we don't find *any*
                # new points (which shouldn't happen, but I suppose might happen
                # because of rounding)
                newFound = True
                while newFound and not (endPoint is perimeter[0]):
                    newFound = False

                    for C in candidates:
                        # don't ever reconsider any point which is already in the perimeter, except
                        # for the case of perimeter[0].  But, of course, even perimeter[0] is only
                        # valid once we have 3 or more points already on the perimeter.

                        if C is perimeter[0]:
                            if len(perimeter) < 3:
#                                print "slice(): SKIPPING perimeter[0], as it is too early to consider closed loops.  perimeter[0] = %s" % perimeter[0]
                                continue
                        else:
                            dup = False
                            for check in perimeter[1:]:
                                if C is check:
                                    dup = True
                                    break
                            if dup:
#                                print "slice(): SKIPPING POINT, DUP!!!  %s" % C
                                continue   # don't consider this point, it's a duplicate


#                        print "slice(): CHECKING ANOTHER POINT: %s" % C

                        if not isPerimeterPair(endPoint,C):
                            continue

#                        print "slice(): NEXT POINT ON PERIMETER FOUND!"

                        newFound = True
                        perimeter.append(C)
                        endPoint = C

                        break  # break out of the for loop, start the next
                               # iteration of the while() loop...unless, of
                               # course, we have looped back to the beginning
                               # of the perimeter.

#                if not newFound:
#                    print "slice(): WARNING!!!      >>> Broke out of loop without closing the loop! <<<"
#                else:
#                    print "slice(): PERIMETER LOOP DETECTED.  Perimeter search complete."

#                print "slice(): PERIMETER: %s" % exp_str(perimeter)
#                print

#                print "slice(): PERIMETER (normalized):"
#                for i in range(0,len(perimeter)-1):
#                    print " %s" % (perimeter[i+1] - perimeter[i]).normalize()

                break # these next 3 lines drop out of the two big for() loops
                      # above, now that we have found a perimeter.
            if len(perimeter) > 0:
                break



        # We have finally found the perimeter of the new face...this tells
        # us the list of points to save.  Note that since this plane actually
        # changed the shape of the set, we save it, too.
        #
        # bugfix: the first and last points on the perimeter are the same.  Don't
        # save it twice.

        self.planes.append(plane)
        self.points = pts_in + perimeter[0: -1] # -1 means "one less than the end"

#        print "slice(): ENDING POINTS: %s" % exp_str(self.points)
#        print

        return

    # END OF FUNCTION: Intersection::slice()


    def intersectWith(self, other_space):
        # this intersects the current object with a complete new space.  It
        # returns a copy of this object.  You are *not* required to duplicate
        # the original before calling this.
        temp_intersection = copy.deepcopy(self)

#        print "Intersection:intersectWith(): other_space=%s" % other_space
#        print

#        print "Intersection:intersectWith(): slicing the old intersection with the new planes"
#        print
        for plane in other_space.planes:
            temp_intersection.slice(plane)
            if temp_intersection.empty():
                break

        # We have now calculated the intersection of our old object with the
        # new space.  (It might be empty).  Record that the new space is
        # part of this intersection before we return the result.
        temp_intersection.convex_spaces.append(other_space)

        return temp_intersection


    def extractSpace(self):
        # this extracts a Convex_Space object from this intersection object.

        retval = Convex_Space()

        retval.planes = self.planes  # TODO: cull planes, someday

        retval.sensor    = "intersector"
        retval.algorithm = "intersector"

        # the aux_payload of the new Convex_Space is the union of all the
        # input payloads, plus the point list and bounding box we have
        # calculated.
        retval.aux_payload = ""
        for space in self.convex_spaces:
#            print "AUX PAYLOAD: " + space.aux_payload
            retval.aux_payload += space.aux_payload + "\n"
#        print
#        print retval.aux_payload
#        print

        # this little loop lists the points we have calculated...
        min_x = max_x = self.points[0].x
        min_y = max_y = self.points[0].y
        min_z = max_z = self.points[0].z

        pts_payload = ""

	global marker # added by Jeremy
        for pt in self.points:
            pts_payload += " (%f,%f,%f)" % (pt.x,pt.y,pt.z)
            marker.points.append(Point(pt.x,pt.y,pt.z)) # added by Jeremy

            if pt.x < min_x:
                min_x = pt.x
            if pt.x > max_x:
                max_x = pt.x

            if pt.y < min_y:
                min_y = pt.y
            if pt.y > max_y:
                max_y = pt.y

            if pt.z < min_z:
                min_z = pt.z
            if pt.z > max_z:
                max_z = pt.z

        bb_payload = "x=(%f,%f) y=(%f,%f) z=(%f,%f)" % (min_x,max_x, min_y,max_y, min_z,max_z)

        """ # added by Jeremy - show only bounding box
        marker.points.append(Point(min_x,min_y,min_z))
        marker.points.append(Point(min_x,min_y,max_z))
        marker.points.append(Point(min_x,max_y,max_z))
        marker.points.append(Point(max_x,max_y,max_z))
        marker.points.append(Point(max_x,max_y,min_z))
        marker.points.append(Point(max_x,min_y,min_z))
        marker.points.append(Point(max_x,min_y,max_z))
        marker.points.append(Point(min_x,max_y,min_z))
        """
        
        retval.aux_payload += "convex_set: %s\n" % pts_payload
        retval.aux_payload += "bounding_box: %s\n" % bb_payload

        # also, add a count of the input spaces as a quick estimate of how
        # interesting this object is.
        retval.aux_payload += "convex_space_count: %d" % len(self.convex_spaces)


        return retval



# MEMORY ALGORITHM
#
# This seemed simple at first, but turned out to be a little trickier than I
# would have guessed.
#
# The issue here is that we are going to keep information about some list of
# old messages, and we need to intersect each with each new message.  The
# problem is, it's not as simple as just performing n intersections.  We
# actually need to check every combination, which means 2^n checks per new
# message!  Not going to work!  (Even culling out spaces which come from the
# same sensor, we still have O(2^s) intersections, where s is the number of
# sensor/algorithm combinations.  Bad story, as we add algorithms.)
#
# What we want to do is to cache all of the old intersection checks we did
# (during previous message checks).  But we also need to expire old
# intersections as the messages get old.  My basic algorithm is to keep an
# intersection around until one of the spaces which it came from gets too old.
# Note that we do *NOT* sort based on the newest space which contributes to the
# intersection; we sort based on the oldest.
#
# The solution is to keep all of the intersections, but to sort them into
# groups based on their expiration time.  Our memory variable, thus, is an
# array of arrays; each element represents a set of spaces which will all
# expire at the same time, because their oldest input space is the same.
#
# Each time that we get a new space, we will crop off the front of this array,
# and intersect the new space with each space we find in the rest of the
# arrays.  The non-empty intersections, if any, will be appended to each list
# in turn; then the new space will be appended (by itself) to the end of the
# list.



# these are the most recent saved spaces.  We choose to save 10 spaces.
memory = []
for i in range(0,10):
    memory.append([])



# by touching this variable here, in the global space, we make sure that the
# variable is in the global namespace.  Therefore, when it is set inside of
# __main__, it will be readable inside of input_handler().
publisher = None


def find_intersections(new_convex_space):
    # This function is called by input_handler() to compare all remembered
    # spaces and intersections to a just-arrived one.


    # sanity check the new space
    for plane in new_convex_space.planes:
        # if the magnitude of the normal is 0, then something is very wrong
        if plane.a*plane.a + plane.b*plane.b + plane.c*plane.c == 0:
            print "3d_intesector:find_intersections: ERROR!  Plane has normal with 0 magnitude!!!  new_convex_space = %s" % new_convex_space
            return []


    # this is the new array of arrays that we're building
    new_mem = []

    # this is the array of new spaces we find, which will need to get reported
    # eventually.
    new_intersections = []


    global memory

#    print "ARGUMENT:"
#    print new_convex_space
#    print

#    print "MEMORY: len(memory)=%d memory=%s" % ( len(memory), memory ) # exp_str(memory, 2) )
#    print

#    print "    new_convex_space.sensor      = '%s'" % new_convex_space.sensor
#    print "    new_convex_space.algorithm   = '%s'" % new_convex_space.algorithm
#    print "    new_convex_space.aux_payload = '%s'" % new_convex_space.aux_payload
#    print

#    print "find_intersections(): new_convex_space=%s" % new_convex_space
#    print

    for group in memory[1:]:
#        print "GROUP:"
#        print group


        # this is the new group that we're building.  We always save the old
        # intersections in the list, before we consider building new ones.
        new_group = []

        for old_intersection in group:
#            print "OLD_INTERSECTION:"
#            print old_intersection


            # save the old intersection before we decide whether we can also intersect
            # with it.
            new_group.append(old_intersection)


            # scan through the list of Convex_Space's that make up this
            # intersection.  Look for any sensor/algorithm pair which matches
            # this one, which would disqualify this intersection from
            # consideration.  Do you worry that we are missing out?  What if
            # there exists another intersection we need to make, with this
            # intersection, less that one conflicting Convex_Space?  It turns out that
            # we don't have to worry about that...if there is something like
            # that, we'll find it, in another generation of intersections, later on.

            disqualified = False
            for old_convex_space in old_intersection.convex_spaces:
                if new_convex_space.sensor == old_convex_space.sensor and new_convex_space.algorithm == old_convex_space.algorithm:
                    disqualified = True
                    break
            if disqualified:
                continue


            # now that we know that this space is qualified for consideration,
            # we actually make the attempt, and see if anything results.
            temp_intersection = old_intersection.intersectWith(new_convex_space)
            if temp_intersection.empty():
                continue


            # since the intersection is non-empty, we record it as a new thing
            # to remember.  Note that we don't report it this instant; instead,
            # we will return all new intersections to input_handler(), which
            # will handle the ROS publication issues.
            new_group        .append(temp_intersection)
            new_intersections.append(temp_intersection)

        # end of the foreach(space) loop

        # when we get here, we have scanned the entire old group and produced
        # new group.  It contains all of the old objects, and perhaps some new
        # ones.
        new_mem.append(new_group)

    # end of foreach(old group) loop


    # build a new group, which has exactly one member: the new space that was
    # given to us.  Also, add this to new_intersections, so that we'll return
    # it to our caller.
    this_intersection = Intersection(new_convex_space)
    if len(this_intersection.points) == 0:
        print "3d_intersector:find_intersections(): ERROR: this_intersection has no points!  new_convex_space = %s" % new_convex_space
        return []  # don't change memory, something is wrong with this space.

    new_intersections.append(this_intersection)

    new_mem.append([this_intersection])


    # ...and then save what we've built as the new memory.
    memory = new_mem


#    print "UPDATED MEMORY: len(memory)=%d memory=%s" % ( len(memory), memory ) # exp_str(memory, 2) )
#    print


    # new_intersections holds the list of new intersections found (from all
    # different groups)
    return new_intersections
# end of function find_intersections



import threading

global input_handler_lock
input_handler_lock = threading.Lock()

def input_handler(new_convex_space):
    global input_handler_lock

#    print "THREAD:              count: %d cur: %s" % (threading.active_count(), threading.current_thread())
    input_handler_lock.acquire()
#    print "THREAD: LOCK GAINED: count: %d cur: %s" % (threading.active_count(), threading.current_thread())


    # this function is called by ROS when a new Convex_Space message arrives.
    #
    # It calls find_intersections() to find new intersections, updating the
    # memory as it goes.  It returns a list of new intersections.  We will
    # then publish those intersections out the other ROS interface.

    global publisher
    global marker
    global marker_pub
    
    marker.points = []
    new_intersections = find_intersections(new_convex_space)

#    print "new_intersections = %s" % exp_str(new_intersections)
#    print

    for new_intersection in new_intersections:
        if len(new_intersection.points) == 0:
            print "3d_intersector:input_handler: ERROR: intersection object has no points!!!  new_intersection = %s" % new_intersection
            continue

        publisher.publish(new_intersection.extractSpace())
        
    marker_pub.publish(marker)

    input_handler_lock.release()



def init():

    # all this about marker added by Jeremy
    global marker
    marker = Marker()
    marker.header.frame_id = "/map"
    marker.header.stamp = rospy.Time()
    marker.ns = ""
    marker.id = 0
    marker.type = 8 # 8 = POINTS
    marker.action = 0 # 0 = ADD
    marker.pose.position.x = 0
    marker.pose.position.y = 0
    marker.pose.position.z = 0
    marker.pose.orientation.x = 0.0
    marker.pose.orientation.y = 0.0
    marker.pose.orientation.z = 0.0
    marker.pose.orientation.w = 0.0
    marker.scale.x = .1
    marker.scale.y = .1
    marker.color.a = 1
    marker.color.r = 1
    marker.color.g = 0
    marker.color.b = 0

    global publisher
    global marker_pub # added by Jeremy

    input_name  = "intersector_input"
    output_name = "intersector_output"

    rospy.init_node('intersector')

    marker_pub = rospy.Publisher('/intersector/markers', Marker) # added by Jeremy
    publisher = rospy.Publisher(output_name, Convex_Space)

    rospy.Subscriber(input_name, Convex_Space, input_handler)



if __name__ == '__main__':
    try:
        init()
        rospy.spin()
    except rospy.ROSInterruptException: pass

